/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.team1160.breakaway.camera;

import com.team1160.breakaway.util.Point2D;
import edu.wpi.first.wpilibj.image.EllipseMatch;
import edu.wpi.first.wpilibj.util.SortedVector;
import java.util.Vector;

/**
 *
 * @author nttoole
 */
public class EllipseCenterPartitioner
{

    public static final double DELTA = Targetting.CENTER_DELTA;


    public EllipseCenterPartitioner()
    {
    
    }

    //---------------------------------------------------------------------
    //---------------------------------------------------------------------

    protected Vector partitionSharedCenters(EllipseMatch[] targets)
    {
        //Vector of Vector of EllipseMatches
        Vector listOfEllipseMatchLists = new Vector();

        int curIndex = 0;

        boolean cont = true;
        for (int i = 0; i < targets.length; ++i)
        {
            EllipseMatch curEllipse = targets[i];

            //check if we have seen this ellipse already
            //if so, then we already have its bigger brother
            //and smaller siblings
            boolean foundIt = false;
            for (int j = 0; !foundIt &&
                     j < listOfEllipseMatchLists.size(); ++j)
            {
                Vector v = (Vector) listOfEllipseMatchLists.elementAt(j);
                if (v.contains(curEllipse))
                {
                    foundIt = true;
                }
            }

            //---------------------

            //if not found, then this is the first time we have
            //seen this ellipse, so lets see if it has any
            //smaller ellipses that have a similar center!
            //otherwise, it has been paired with a larger
            //ellipse already
            if (!foundIt)
            {

                //because we are passing 'i' instead of 'i+1',
                //the matches will automaticallly include the
                //curEllipse
                Vector matches = getSubsetWithSameCenterFromIndex(
                                    curEllipse, targets, i);

                //therefore, we need to check if it contains more
                //than one item to count as a match
                if (matches.size() > 1)
                {
                    listOfEllipseMatchLists.addElement(matches);
                }
            } //end_not_found_it

        }



        return listOfEllipseMatchLists;
    }

    //---------------------------------------------------------------------

    /**
     * Moves down the rest of the targets array looking for
     * matching centers.  If an ellipse matches, it is added
     * to the returned vector.  NOTE: vector does NOT contain the
     * curEllipse by default.
     * @param curEllipse Primary ellipse
     * @param targets Array of EllipseMatches to be searched
     * @param startIndex Start index
     * @return Vector of all ellipses with similar center
     * onward from startIndex
     */

    protected Vector getSubsetWithSameCenterFromIndex(
                                   EllipseMatch curEllipse,
                                   EllipseMatch[] targets,
                                   int startIndex)
    {
        Vector vector = new SortedVector(new EllipseAreaComparator());

        for (int i = startIndex; i < targets.length; ++i)
        {
            EllipseMatch otherEllipse = targets[i];

            Point2D centerPt = getSameCenter(curEllipse,
                                           otherEllipse);

            if (centerPt != null)
            {
                if (!vector.contains(otherEllipse))
                    vector.addElement(otherEllipse);
            }
        }

        return vector;
    }

    //---------------------------------------------------------------------

    /**
     * Tests is both ellipses share the same center (within
     * a pre-defined delta).  If so, then a combined center
     * is computed and returned.  Otherwise, null is returned,
     * @param matchOne An EllipseMatch instance
     * @param matchTwo An EllipseMatch instance
     * @return New Point2D with center, null if not found.
     */

    protected Point2D getSameCenter(EllipseMatch matchOne,
                                    EllipseMatch matchTwo)
    {
        boolean areSame = false;
        Point2D sharedCenter = null;

         if((Math.abs(matchOne.m_xPos - matchTwo.m_xPos) < DELTA) &&
            (Math.abs(matchOne.m_yPos - matchTwo.m_yPos) < DELTA))
         {

             areSame = true;
             sharedCenter = getAverageCenter(matchOne, matchTwo);
         }

        return sharedCenter;
    }

    //---------------------------------------------------------------------

    /**
     * Convenience method that returns the average center of two
     * ellipses
     * @param matchOne
     * @param matchTwo
     * @return Average center
     */

    protected Point2D getAverageCenter(EllipseMatch matchOne,
                                    EllipseMatch matchTwo)
    {
        Vector tmpVec = new Vector();
        tmpVec.addElement(matchOne);
        tmpVec.addElement(matchTwo);

        return getAverageCenter(tmpVec);
    }

    //---------------------------------------------------------------------

    /**
     * Calculates the average center based on all of the ellipses
     * in the argument
     * @param ellipses Vector of EllipseMatch instances
     * @return Average center
     */

    protected Point2D getAverageCenter(Vector ellipses)
    {
        int count = 0;
        double x_val_sum = 0.0;
        double y_val_sum = 0.0;
        EllipseMatch ellipse;
        Point2D centerPt = null;

        for (int i = 0; i < ellipses.size(); ++i)
        {
             ellipse = (EllipseMatch) ellipses.elementAt(i);
             if (ellipse == null)
                 continue;

             ++count;
             x_val_sum += ellipse.m_xPos;
             y_val_sum += ellipse.m_yPos;

        }

        if (count > 0)
        {
            int x_val_avg = (int) (x_val_sum / count);
            int y_val_avg = (int) (y_val_sum / count);
            centerPt = new Point2D(x_val_avg, y_val_avg);

        }

        return centerPt;
    }
}
